/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.RobotMap;

/**
 *
 * @author MattKahn
 */
public class PneumaticSwerveSubsystem extends SwerveSubsystem {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    public PneumaticSwerveSubsystem() {
        System.out.println("Constructing PneumaticSwerveSubsystem");
        
        Compressor compressor = new Compressor(RobotMap.kPressureSwitchGPIOChannel, RobotMap.kCompressorRelayChannel);
        compressor.start();

        swerveRetract = new Solenoid(RobotMap.kSolenoidRetract);
        swerveDeploy = new Solenoid(RobotMap.kSolenoidDeploy);        
    }

    
        public double getPosition()
    {
        if(isDeployed())
        {
            return 1;
        }
        else
        {
            return 0;
        }
    }

    public double getCommandedPosition() {
        return getPosition();
    }

    public void setPosition(double position) {
    // Do nothing - this makes no sense for the pneumatic swerve
    }
    
            
    public void raiseSwerve() {
        System.out.println("PneumaticSwerveSubsystem: Raise Swerve");
        if ((swerveRetract != null) && (swerveDeploy != null)) {
            swerveRetract.set(true);
            swerveDeploy.set(false);
            m_kSwerveStateDeployed = false;
        }
    }

    public void lowerSwerve() {
        System.out.println("PneumaticSwerveSubsystem: Lower Swerve");
        if ((swerveRetract != null) && (swerveDeploy != null)) {
            swerveRetract.set(false);
            swerveDeploy.set(true);
            m_kSwerveStateDeployed = true;
        }
    }
    
    public void recover(boolean override) {
        // No recovery actions needed for Pneumatic swerve
    }
}
